
#include "Warpping.h"
using namespace windage;

void Warpping::Initialize(int width, int height)
{
	this->width = width;
	this->height = height;

	if(dstMapX) cvReleaseImage(&dstMapX);
	if(dstMapY) cvReleaseImage(&dstMapY);

	dstMapX = cvCreateImage(cvSize(width, height), IPL_DEPTH_32F, 1);
	dstMapY = cvCreateImage(cvSize(width, height), IPL_DEPTH_32F, 1);
}

bool Warpping::UpdateWarppingParameter(bool undistortion, windage::Matrix3* homography)
{
	if(calibration == NULL || dstMapX == NULL || dstMapY == NULL)
		return false;

	if(undistortion)
	{
		IplImage* distortionMapX = cvCreateImage(cvSize(width, height), IPL_DEPTH_32F, 1);
		IplImage* distortionMapY = cvCreateImage(cvSize(width, height), IPL_DEPTH_32F, 1);
		cvInitUndistortMap(calibration->GetIntrinsicMatrix(), calibration->GetDistortionCoefficients(), distortionMapX, distortionMapY);

		for(int y=0; y<height; y++)
		{
			for(int x=0; x<width; x++)
			{
				double dx = x + translationX;
				double dy = height - y - 1 + translationY;
				dx /= scale;
				dy /= scale;

				CvPoint point;
				if(homography)
				{
					windage::Vector3 tempPt = windage::Vector3(dx, dy, 1.0);
					windage::Vector3 target = (*homography) * tempPt;
					target /= target.z;

					point.x = cvRound(target.x);
					point.y = cvRound(target.y);
				}
				else
				{
					point = calibration->ConvertWorld2Image(dx, dy, 0.0);
				}

				if( 0 <= point.x && point.x < width &&
					0 <= point.y && point.y < height)
				{
					double tx = cvGetReal2D(distortionMapX, point.y, point.x);
					double ty = cvGetReal2D(distortionMapY, point.y, point.x);

					cvSetReal2D(dstMapX, y, x, (double)tx);
					cvSetReal2D(dstMapY, y, x, (double)ty);
				}
				else
				{
					cvSetReal2D(dstMapX, y, x, -1.0);
					cvSetReal2D(dstMapY, y, x, -1.0);
				}
			}
		}
 
		cvReleaseImage(&distortionMapX);
		cvReleaseImage(&distortionMapY);
	}
	else
	{
		for(int y=0; y<height; y++)
		{
			for(int x=0; x<width; x++)
			{
				double dx = x + translationX;
				double dy = height - y - 1 + translationY;
				
				CvPoint point;
				if(homography)
				{
					windage::Vector3 tempPt = windage::Vector3(dx, dy, 1.0);
					windage::Vector3 target = (*homography) * tempPt;
					target /= target.z;

					point.x = cvRound(target.x);
					point.y = cvRound(target.y);
				}
				else
				{
					point = calibration->ConvertWorld2Image(dx, dy, 0.0);
				}

				if( 0 <= point.x && point.x < width &&
					0 <= point.y && point.y < height)
				{
					cvSetReal2D(dstMapX, y, x, (double)point.x);
					cvSetReal2D(dstMapY, y, x, (double)point.y);
				}
				else
				{
					cvSetReal2D(dstMapX, y, x, -1.0);
					cvSetReal2D(dstMapY, y, x, -1.0);
				}
			}
		}
	}
	return true;
}

bool Warpping::Do(IplImage* src, IplImage* dst)
{
	if(calibration == NULL || dstMapX == NULL || dstMapY == NULL)
		return false;

	if(src->nChannels != dst->nChannels)
		return false;

	cvRemap(src, dst, dstMapX, dstMapY, CV_INTER_LINEAR+CV_WARP_FILL_OUTLIERS);
	return true;
}